#include "code_read_camera/code_read_camera.h"
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <eigen3/Eigen/Eigen>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <shared_parameters/shared_params_interface.hpp>

using namespace Eigen;

CodeReadCamera::CodeReadCamera() 
    : rclcpp::Node("code_read_camera"),
      earth_cam_node_id_(0),
      shelf_cam_node_id_(0),
      init_step_(0),
      camera_latency_(0),
      use_pdo_timestamp_(true) 
{
  initialize();
}

CodeReadCamera::~CodeReadCamera()
{
  if (processing_thread_.joinable()) {
    processing_thread_.join();
  }
}

void CodeReadCamera::initialize()
{
  setup_parameters();
  init_devices();
  setup_publishers();
  start_processing_thread();
}

void CodeReadCamera::start_processing_thread()
{
  RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), "Start processing thread.");
  processing_thread_ = std::thread([this]() {
    rclcpp ::Rate rate(100);
    while (rclcpp::ok()) {
      process_qr_data(); // update
      rate.sleep();
    }
  });
}

void CodeReadCamera::setup_publishers()
{
  qr_code_group_pub_ = create_publisher<code_read_camera_msgs::msg::QrCodeinfoGroup>("QRcode_Info", 50);
  landmark_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("landmark", 1);
  slam_landmark_pub_ = create_publisher<slam_ros_msgs::msg::QRCode>("/qr_code", 1);
  shelf_landmark_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("/landmark_shelf", 1);
  // 2023.3.20
  ori_landmark_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("origin_landmark", 1);
  event_pub_ = create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 1);
}

void CodeReadCamera::process_qr_data()
{
  std::lock_guard<std::mutex> lock(data_mutex_);

  for (int i = 0; i < node_ids_.size(); i++) {
    qr_code_info_vect_[i].nodeid = node_ids_[i];
    qr_code_info_vect_[i].xpos = qr_code_info_[node_ids_[i]].xpos;
    qr_code_info_vect_[i].ypos = qr_code_info_[node_ids_[i]].ypos;
    qr_code_info_vect_[i].angle = qr_code_info_[node_ids_[i]].angle;
    qr_code_info_vect_[i].tag = qr_code_info_[node_ids_[i]].tag;
    qr_code_info_vect_[i].timestamp = qr_code_info_[node_ids_[i]].timestamp;
    qr_code_info_vect_[i].update_timestamp = qr_code_info_[node_ids_[i]].update_timestamp;
  }
  
  qr_code_info_group_.qr_code_info_group = qr_code_info_vect_;
  qr_code_group_pub_->publish(qr_code_info_group_);
  
  // 地面二维码数据
  if (earth_cam_node_id_ > 0 && qr_code_info_.count(earth_cam_node_id_)) {
    const auto& info = qr_code_info_[earth_cam_node_id_];
    RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), 
      "Xpos: %d  Ypos:%d  angle: %d  tag: %d", 
      info.xpos, info.ypos, info.angle, info.tag);
    
    if (validate_camera_data(info)) {
      geometry_msgs::msg::PoseStamped landmark;
      slam_ros_msgs::msg::QRCode slam_landmark;
      convert_to_landmark(info, landmark, slam_landmark);
      // 增加二维码x和y数据阈值判断
      if (validate_landmark(landmark)) {
        landmark_pub_->publish(landmark);
      }

      slam_landmark_pub_->publish(slam_landmark);
      // 2023.3.20
      ori_landmark_pub_->publish(ori_landMark_);
    }
  }
  // 上读头二维码数据
  if (shelf_cam_node_id_ > 0 && qr_code_info_.count(shelf_cam_node_id_)) {
    const auto& info = qr_code_info_[shelf_cam_node_id_];
    if (validate_camera_data(info)) {
      geometry_msgs::msg::PoseStamped landmark;
      convert_to_shelf_landmark(info, landmark);
      shelf_landmark_pub_->publish(landmark);
    }
  }
}

// 2025.3.20 
void CodeReadCamera::setup_pdo_subscriptions(int node_id, bool is_downward) 
{
  auto create_sub = [&](auto&&... args) {
    return this->create_subscription<std_msgs::msg::UInt64>(args...);
  };

  const std::string device_name = "/" + can_device_ + "/device";
  const std::string base_topic = device_name + std::to_string(node_id);
  
  auto bind_callback = [this](auto callback) {
      return std::bind(callback, this, std::placeholders::_1);
  };

  if (is_downward) {
    downward_pdo_subs_ = {
      create_sub(base_topic + "/get_pdo_raw_data_0", 10, bind_callback(&CodeReadCamera::downward_pdo0_sub_callback)),
      create_sub(base_topic + "/get_pdo_raw_data_1", 10, bind_callback(&CodeReadCamera::downward_pdo1_sub_callback)),
      create_sub(base_topic + "/get_pdo_raw_data_2", 10, bind_callback(&CodeReadCamera::downward_pdo2_sub_callback)),
      create_sub(base_topic + "/get_pdo_raw_data_3", 10, bind_callback(&CodeReadCamera::downward_pdo3_sub_callback))
    };

    if (use_pdo_timestamp_) {
      auto ts_callback = std::bind(&CodeReadCamera::downward_pdo2_timestamp_sub_callback, this, std::placeholders::_1);
      downward_pdo2_ts_sub_ = this->create_subscription<std_msgs::msg::Float64>(
        base_topic + "/get_pdo_raw_data_2/timestamp", 10, ts_callback);
    }
  } else {
    upward_pdo_subs_ = {
      create_sub(base_topic + "/get_pdo_raw_data_0", 10, bind_callback(&CodeReadCamera::upward_pdo0_sub_callback)),
      create_sub(base_topic + "/get_pdo_raw_data_1", 10, bind_callback(&CodeReadCamera::upward_pdo1_sub_callback)),
      create_sub(base_topic + "/get_pdo_raw_data_2", 10, bind_callback(&CodeReadCamera::upward_pdo2_sub_callback)),
      create_sub(base_topic + "/get_pdo_raw_data_3", 10, bind_callback(&CodeReadCamera::upward_pdo3_sub_callback))
    };

    if (use_pdo_timestamp_) {
      auto ts_callback = std::bind(&CodeReadCamera::upward_pdo2_timestamp_sub_callback, this, std::placeholders::_1);
      upward_pdo2_ts_sub_ = this->create_subscription<std_msgs::msg::Float64>(
        base_topic + "/get_pdo_raw_data_2/timestamp", 10, ts_callback);
    }
  }
}

void CodeReadCamera::init_devices() 
{
  this->get_parameter_or("can_device", can_device_, std::string("can0")); 
  this->get_parameter_or("camera_latency", camera_latency_, 0);
  this->get_parameter_or("use_pdo_timestamp", use_pdo_timestamp_, true);
  RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), 
    "get camera_latency: %dms  use_pdo_timestamp: %d", camera_latency_, use_pdo_timestamp_);

  if (init_device(earth_cam_node_id_, "earth_code_camera")) {
    setup_pdo_subscriptions(earth_cam_node_id_, true);
  }

  if (init_device(shelf_cam_node_id_, "shelf_code_camera")) {
    setup_pdo_subscriptions(shelf_cam_node_id_, false);
  }

  if (qr_code_info_.empty()) {
    RCLCPP_ERROR(rclcpp::get_logger("CodeReadCamera"), "No valid node configured");
  }

  qr_code_info_vect_.resize(node_ids_.size());
}

bool CodeReadCamera::init_device(int& node_id, const std::string& node_name) 
{
  if (!get_parameter(node_name + "_nodeId", node_id)) {
    RCLCPP_WARN(rclcpp::get_logger("CodeReadCamera"), "%s node not configured", node_name.c_str());
    return false;
  }
  
  QRCodeInfo QR_code_info = {0};
  qr_code_info_.insert(std::pair<int, QRCodeInfo>(node_id, QR_code_info));
  node_ids_.push_back(node_id);
  // nodes_name_.push_back(node_name);
  return true;
}

bool CodeReadCamera::validate_camera_data(const QRCodeInfo& info) const
{
  return !(info.tag == 0 || (info.xpos == 0 && info.angle == 0) || info.ypos == 0);
}

//
// 下读头二维码消息发布 chang by xlb
//
void CodeReadCamera::convert_to_landmark(const QRCodeInfo& info,
      geometry_msgs::msg::PoseStamped& landmark,
      slam_ros_msgs::msg::QRCode& slam_landmark)
{
  Matrix3f T_QrToCamera, T_CameraToQr, T_BaseToCamera, T_CameraToBase, T_QrToBase, T_BaseToQr;
  Matrix3d BaseToQr_RM;

  // 2023.3.21
  float TcX = this->get_parameter_or("TcX", 0.0);
  float TcY = this->get_parameter_or("TcY", 0.0);
  float TcR = this->get_parameter_or("TcR", 0.0);
  RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), "TcX:%f, TcY:%f, TcR:%f", TcX, TcY, TcR);

  T_BaseToCamera << cosf(-TcR), sinf(-TcR), -TcX,
      -sinf(-TcR), cosf(-TcR), -TcY,
      0, 0, 1;

  float xpos = info.xpos / 10000.0;
  float ypos = info.ypos / 10000.0;
  float apos = (-info.angle) * (M_PI / 1800.0);

  T_QrToCamera << cosf(-apos), sinf(-apos), -xpos,
      -sinf(-apos), cosf(-apos), -ypos,
      0, 0, 1;
  T_CameraToQr = T_QrToCamera.inverse();
  T_BaseToQr = T_BaseToCamera * T_CameraToQr;
  
  // RCLCPP_INFO_STREAM(rclcpp::get_logger("CodeReadCamera"), "camera2base:\n" << T_BaseToCamera);
  // RCLCPP_INFO_STREAM(rclcpp::get_logger("CodeReadCamera"), "camera2qr:\n" << T_QrToCamera);
  // RCLCPP_INFO_STREAM(rclcpp::get_logger("CodeReadCamera"), "base2qr:\n" << T_BaseToQr);
  
  // change by yxj
  T_QrToBase = T_BaseToQr.inverse();
  Matrix3d QrToBase_RM;

  QrToBase_RM << T_QrToBase(0, 0), T_QrToBase(0, 1), 0,
      T_QrToBase(1, 0), T_QrToBase(1, 1), 0,
      0, 0, 1;

  Matrix3d T_BaseToQr_RM;
  T_BaseToQr_RM << T_BaseToQr(0, 0), T_BaseToQr(0, 1), 0,
      T_BaseToQr(1, 0), T_BaseToQr(1, 1), 0,
      0, 0, 1;

  ::Quaterniond QtoR = ::Quaterniond(QrToBase_RM);
  QtoR.normalize();

  ::Quaterniond BTL = ::Quaterniond(T_BaseToQr_RM);
  BTL.normalize();
 
  if (use_pdo_timestamp_) {
    try {
      landmark.header.stamp = this->now() - rclcpp::Duration::from_seconds(camera_latency_ / 1000.0);
      RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), 
        "update_timestamp: %.4f", info.update_timestamp);
    } catch (...) {
      landmark.header.stamp = this->now();
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("CodeReadCamera"), 
        "error:crash: current landmark time is error");
    }
  } else {
    landmark.header.stamp = this->now();
  }

  landmark.header.frame_id = "QR"; //"base_link";
  // landmark.header.seq = info.tag;
  landmark.pose.position.x = -T_QrToBase(0, 2);
  landmark.pose.position.y = -T_QrToBase(1, 2);
  landmark.pose.position.z = info.tag;
  landmark.pose.orientation.w = QtoR.w();
  landmark.pose.orientation.x = 0;
  landmark.pose.orientation.y = 0;
  landmark.pose.orientation.z = QtoR.z();
  double siny_cosp = 2 * (QtoR.w() * QtoR.z() + QtoR.x() * QtoR.y());
  double cosy_cosp = 1 - 2 * (QtoR.y() * QtoR.y() + QtoR.z() * QtoR.z());
  double yaw = std::atan2(siny_cosp, cosy_cosp) * 180 / M_PI;
  RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), 
    "landmark's info : tag = %d  x = %lf y = %lf yaw = %lf", info.tag, xpos, ypos, yaw);

  slam_landmark.header.stamp = this->now();
  slam_landmark.header.frame_id = "base_link";
  // slam_landmark.header.seq = info.tag;
  slam_landmark.pose.position.x = T_QrToBase(0, 2);
  slam_landmark.pose.position.y = T_QrToBase(1, 2);
  slam_landmark.pose.position.z = info.tag;
  slam_landmark.pose.orientation.w = BTL.w();
  slam_landmark.pose.orientation.x = 0;
  slam_landmark.pose.orientation.y = 0;
  slam_landmark.pose.orientation.z = BTL.z();
  slam_landmark.id = std::to_string(info.tag);

  // 2023.3.20
  // ori_landMark_.header.stamp = this->now();
  if (use_pdo_timestamp_) {
    try {
      ori_landMark_.header.stamp = rclcpp::Time(info.update_timestamp);
    } catch (...) {
      ori_landMark_.header.stamp = this->now();
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("CodeReadCamera"), 
        "error:crash: current ori_landMark_ time is error");
    }
  } else {
    ori_landMark_.header.stamp = this->now();
  }

  ori_landMark_.header.frame_id = "QRcode";
  ori_landMark_.pose.position.x = xpos;
  ori_landMark_.pose.position.y = ypos;
  ori_landMark_.pose.position.z = info.tag;
 
  // ori_landMark_.pose.orientation = tf::createQuaternionMsgFromYaw(apos);
  tf2::Quaternion q;
  q.setRPY(0, 0, apos); 
  ori_landMark_.pose.orientation = tf2::toMsg(q); 
}

bool CodeReadCamera::validate_landmark(const geometry_msgs::msg::PoseStamped& msg) const
{
  const float valid_threshold = this->get_parameter_or("valid_threshold", 0.065f);
  const float x = msg.pose.position.x;
  const float y = msg.pose.position.y;
    
  if (std::abs(x) > valid_threshold || std::abs(y) > valid_threshold) {
    RCLCPP_WARN(rclcpp::get_logger("CodeReadCamera"), 
      "Invalid landmark position: x=%.3f, y=%.3f", x, y);
    return false;
  }
  return true;
}

//
// 上读头二维码消息发布
// T_BaseToCamera: camera 相对 base
// add by yxj
void CodeReadCamera::convert_to_shelf_landmark(const QRCodeInfo& info,
      geometry_msgs::msg::PoseStamped& landmark)
{
  Matrix3f T_QrToCamera, T_CameraToQr, T_BaseToCamera, T_CameraToBase, T_QrToBase, T_BaseToQr;
  Matrix3d BaseToQr_RM;

  // 2023.3.21
  float TcX_Shelf = this->get_parameter_or("TcXshelf",  0.0);
  float TcY_Shelf = this->get_parameter_or("TcYshelf",  0.0);
  float TcR_Shelf = this->get_parameter_or("TcRshelf", -0.5 * M_PI);
  
  RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), "TcXshelf:%f, TcYshelf:%f, TcRshelf:%f", TcX_Shelf, TcY_Shelf, TcR_Shelf);

  T_BaseToCamera << cosf(-TcR_Shelf), sinf(-TcR_Shelf), -TcX_Shelf,
      -sinf(-TcR_Shelf), cosf(-TcR_Shelf), -TcY_Shelf,
      0, 0, 1;

  float xpos = info.xpos / 10000.0;
  float ypos = -info.ypos / 10000.0;
  float apos = (info.angle) * (M_PI / 1800.0);

  T_QrToCamera << cosf(-apos), sinf(-apos), -xpos,
      -sinf(-apos), cosf(-apos), -ypos,
      0, 0, 1;
  T_CameraToQr = T_QrToCamera.inverse();

  T_BaseToQr = T_BaseToCamera * T_CameraToQr;

  // change by yxj
  T_QrToBase = T_BaseToQr.inverse();

  Matrix3d QrToBase_RM;

  QrToBase_RM << T_QrToBase(0, 0), T_QrToBase(0, 1), 0,
      T_QrToBase(1, 0), T_QrToBase(1, 1), 0,
      0, 0, 1;

  ::Quaterniond QtoR = ::Quaterniond(QrToBase_RM);
  QtoR.normalize();

  landmark.header.stamp = this->now();
  landmark.header.frame_id = "QR_Shelf";
  // landmark.header.seq = info.tag;
  landmark.pose.position.x = -T_QrToBase(0, 2);
  landmark.pose.position.y = -T_QrToBase(1, 2);
  landmark.pose.position.z = info.tag;
  landmark.pose.orientation.w = QtoR.w();
  landmark.pose.orientation.x = 0;
  landmark.pose.orientation.y = 0;
  landmark.pose.orientation.z = QtoR.z();
  double siny_cosp = 2 * (QtoR.w() * QtoR.z() + QtoR.x() * QtoR.y());
  double cosy_cosp = 1 - 2 * (QtoR.y() * QtoR.y() + QtoR.z() * QtoR.z());
  double yaw = std::atan2(siny_cosp, cosy_cosp) * 180 / M_PI;

  RCLCPP_INFO(rclcpp::get_logger("CodeReadCamera"), 
    "landmarkShelf's info : tag = %d  x = %lf y = %lf yaw = %lf", info.tag, xpos, ypos, yaw);
}

// bytes reversed in topic data
void CodeReadCamera::downward_pdo0_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[earth_cam_node_id_].ypos = extract_value<int32_t>(msg->data, {0, 8, 16, 24});
}

void CodeReadCamera::downward_pdo1_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[earth_cam_node_id_].xpos = extract_value<int32_t>(msg->data, {0, 8, 16, 24});
  qr_code_info_[earth_cam_node_id_].angle = extract_value<int16_t>(msg->data, {32, 40});
}

void CodeReadCamera::downward_pdo2_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[earth_cam_node_id_].tag = extract_value<uint32_t>(msg->data, {32, 40, 48, 56});
}

void CodeReadCamera::downward_pdo3_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[earth_cam_node_id_].timestamp = extract_value<uint32_t>(msg->data, {24, 16, 8, 0});
}

void CodeReadCamera::downward_pdo2_timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr &msg)
{
  qr_code_info_[earth_cam_node_id_].update_timestamp = msg->data;
}

void CodeReadCamera::upward_pdo0_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[shelf_cam_node_id_].ypos = extract_value<int32_t>(msg->data, {0, 8, 16, 24});
}

void CodeReadCamera::upward_pdo1_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[shelf_cam_node_id_].xpos = extract_value<int32_t>(msg->data, {0, 8, 16, 24});
  qr_code_info_[shelf_cam_node_id_].angle = extract_value<int16_t>(msg->data, {32, 40});
}

void CodeReadCamera::upward_pdo2_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[shelf_cam_node_id_].tag = extract_value<uint32_t>(msg->data, {32, 40, 48, 56});
}

void CodeReadCamera::upward_pdo3_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr &msg)
{
  qr_code_info_[shelf_cam_node_id_].timestamp = extract_value<uint32_t>(msg->data, {24, 16, 8, 0});
}

void CodeReadCamera::upward_pdo2_timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr &msg)
{
  qr_code_info_[shelf_cam_node_id_].update_timestamp = msg->data;
}

void CodeReadCamera::setup_parameters() 
{
  if (!SharedParamsInterface::initialize()) {
    RCLCPP_ERROR(get_logger(), "Failed to initialize shared memory interface");
    return;
  }

  try { 
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      RCLCPP_ERROR_STREAM(get_logger(), "Error get shared parameters");
      return;
    } 

    params = SharedParamsInterface::getNodeParams("code_read_camera");
    for (const auto& [name, value] : params) {
      RCLCPP_INFO_STREAM(get_logger(), 
          "code_read_camera:" << name << ", value:" << value.toString());
      switch(value.type) {
        case SharedParamsInterface::ParamValue::Type::INT:
            this->declare_parameter(name, value.getValue<int>());
            break;
        case SharedParamsInterface::ParamValue::Type::DOUBLE:
            this->declare_parameter(name, value.getValue<double>());
            break;
        case SharedParamsInterface::ParamValue::Type::STRING:
            this->declare_parameter(name, value.getValue<std::string>());
            break;
        case SharedParamsInterface::ParamValue::Type::BOOL:
            this->declare_parameter(name, value.getValue<bool>());
            break;
        default:
            RCLCPP_WARN_STREAM(get_logger(), 
              "Unsupported parameter type for " << name);
            break;
      }
    }
  } catch (const std::exception& e) {
    RCLCPP_ERROR_STREAM(get_logger(), 
      "Error reading parameters: " << e.what());
  }
}

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<CodeReadCamera>();
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
  } catch (const std::exception& e) {
      RCLCPP_FATAL(rclcpp::get_logger("CodeReadCamera"), "Fatal error: %s", e.what());
      return EXIT_FAILURE;
  }
  
  rclcpp::shutdown();
  return EXIT_SUCCESS;
}